#include "TrajectoryGenerator.h"
#include <iostream>
#include <string>
#include <cmath>

using namespace std;
using namespace Eigen;

TrajectoryGenerator::TrajectoryGenerator() {}

TrajectoryGenerator::~TrajectoryGenerator() {}

void TrajectoryGenerator::setState(const Matrix<double, 4, 3> &p_stend_O) {
  // 设置状态
  p_stend_O_ = p_stend_O;
}

void TrajectoryGenerator::setDesired(const double &v_x, const double &v_y,
                                     const double &omega_z) {
  // 设置期望值
  v_x_d_B_ = v_x;
  v_y_d_B_ = v_y;
  omega_z_d_B_ = omega_z;

  setflag_ = 1;
  // TODO:检测输入不变时不重复计算
}

void TrajectoryGenerator::calculateAll(const Matrix3d &Rot) {
  // 确保已设置输入
  if (setflag_ != 1) {
    cerr << "TrajectoryGenerator: set expection before get result!" << endl;
    exit(1);
  }

  int p_h = PREDICTION_HORIZON;

  // 初始化 v_1_com_d_O -> D_({9,10,11})
  Vector3d v_k_com_d_B;
  v_k_com_d_B << v_x_d_B_, v_y_d_B_, 0;

  double omega_k_z_d_O = omega_z_d_B_;
  double psi_0_d = atan(Rot(1, 0) / Rot(0, 0));
  double psi_1_d = psi_0_d + omega_k_z_d_O * DELTA_T_MPC;

  D_.block<3, 1>(9, 0) = R("z", psi_1_d) * v_k_com_d_B;

  // 初始化 p_1_com_d_O_ -> D_({3,4,5})
  Vector3d p_1_com_d_O;
  p_1_com_d_O = block_para_ * p_0_com_d_O_ + (1 - block_para_) * p_0_com_O_ +
                v_0_com_d_O_ * DELTA_T_MPC;

  D_.block<3, 1>(3, 0) = p_1_com_d_O;

  // 初始化 omega_1_d_O_ -> D_({6,7,8})
  D_.block<3, 1>(6, 0) << 0, 0, omega_k_z_d_O;

  // 初始化 THETA_1_d_ -> D_({0,1,2})
  // 坡度估计
  Vector4d z_f_O;
  z_f_O << p_stend_O_(0, 2), p_stend_O_(1, 2), p_stend_O_(2, 2),
      p_stend_O_(3, 2);
  Matrix<double, 4, 3> W_pla;

  W_pla << 1, p_stend_O_(0, 0), p_stend_O_(0, 1), 1, p_stend_O_(1, 0),
      p_stend_O_(1, 1), 1, p_stend_O_(2, 0), p_stend_O_(2, 1), 1,
      p_stend_O_(3, 0), p_stend_O_(3, 1);

  Vector3d A_pla;
  A_pla = W_pla.completeOrthogonalDecomposition().pseudoInverse() * z_f_O;

  double yita = 0.2; // 低通滤波系数
  // 低通滤波，利用上一周期的估测值平滑结果
  A_pla = yita * A_pla + (1 - yita) * A_pla_hat_;
  A_pla_hat_ = A_pla; // 更新估计值

  Vector3d n_e;
  n_e << -A_pla(1), -A_pla(2), 1;
  n_e = n_e / n_e.norm(); // 得到长度为1的平面法向量

  // cout<<"n_e:"<<n_e<<endl;
  Matrix3d Gamma;
  Gamma << cos(psi_1_d), sin(psi_1_d), 0, sin(psi_1_d), -cos(psi_1_d), 0, 0, 0,
      1;

  Vector3d zeta = Gamma.completeOrthogonalDecomposition().pseudoInverse() * n_e;
  double fai_1_d = asin(zeta(1));
  double theta_1_d = atan(zeta(0) / zeta(2));

  // cout<<"fai_1_d:"<<fai_1_d<<endl;
  // cout<<"theta_1_d:"<<theta_1_d<<endl;
  // D_({0, 1, 2}) << fai_1_d, theta_1_d, psi_1_d;
  D_.block<3, 1>(0, 0) << fai_1_d, theta_1_d, psi_1_d;

  D_(12) = -9.8;

  double psi_k_d;
  for (int i = 1; i < p_h; i++) {
    psi_k_d = D_(2 + 13 * (i - 1)) + omega_k_z_d_O * DELTA_T_MPC;
    // 计算2~h周期所有v_k_com_d_O_
    D_.block<3, 1>(9 + 13 * i, 0) = R("z", psi_k_d) * v_k_com_d_B;

    // 计算2~h周期所有p_k_com_d_O_
    D_.block<3, 1>(3 + 13 * i, 0) =
        D_.block<3, 1>(3 + 13 * (i - 1), 0) +
        D_.block<3, 1>(9 + 13 * (i - 1), 0) * DELTA_T_MPC;

    // 计算2~h周期所有omega_k_d_O_
    D_.block<3, 1>(6 + 13 * i, 0) << 0, 0, omega_k_z_d_O;

    // 计算2~h周期所有THETA_k_d_
    D_.block<3, 1>(0 + 13 * i, 0) << fai_1_d, theta_1_d, psi_k_d;

    D_(12 + 13 * i) = -9.8;
  }
}

void TrajectoryGenerator::getD(
    Matrix<double, 13 * PREDICTION_HORIZON, 1> &vectorForD) {
  vectorForD = D_;
}

void TrajectoryGenerator::getpsi_k_d(
    Matrix<double, PREDICTION_HORIZON, 1> &vectorForpsi_k_d) {
  for (int i = 0; i < PREDICTION_HORIZON; i++) {
    vectorForpsi_k_d(i, 0) = D_(2 + 13 * i, 0);
  }
}

// Vector3d *TrajectoryGenerator::getv_k_com_d_O(int k, Matrix3d *Rot)
// {
//     //确保已设置输入
//     if (setflag_ != 1)
//     {
//         cerr << "Expected speed not set!";
//         exit(1);
//     }

//     double psi_0_d = atan(((*Rot)(1, 0)) / ((*Rot)(0, 0)));
//     double psi_k_d = 0.0;
//     double omega_k_z_d_O = omega_z_d_B_;

//     for (int i = k_; i < k; i++)
//     {
//         psi_k_d = psi_k_d + omega_k_z_d_O * DELTA_T_MPC;
//     }

//     Vector3d v_k_com_d_B;
//     v_k_com_d_B << v_x_d_B_, v_y_d_B_, 0;

//     Matrix3d tmpmat;
//     tmpmat.setIdentity();
//     (*v_k_com_d_O_) = R"z", psi_k_d) * v_k_com_d_B;

//     return v_k_com_d_O_;
// }

// Vector3d *TrajectoryGenerator::getp_k_com_d_O(int k, Vector3d *p_0_com_O,
// Matrix3d *Rot)
// {

// }
